#include <GL/freeglut.h>
#include <opencv2/opencv.hpp>

#include "hvrl/tools/CVToGLMatrixConversion.hpp"

#include <glm/gtc/type_ptr.hpp>

cv::Size windowsize(800,600);

cv::VideoCapture cap;
cv::Mat frame;

/// Size of the chessboard pattern
cv::Size pattern = cv::Size(7,4);
float squareSize = 0.03f;

unsigned int frametexid = 0;

glm::mat4 glprojection;
glm::mat4 glmodelview;

float near = 0.01f;
float far = 100.0f;
bool calibrated = false;
cv::Matx33f K;
cv::Matx33f R;
cv::Vec3f T;
cv::Mat distCoeffs;
std::vector<std::vector<cv::Point2f> > points;

std::vector<std::vector<cv::Point3f> > corners(1);

unsigned int captured = 0;

bool found = false;
bool isrunning = true;


int timebase = 0;

void drawFullScreenQuad(const unsigned int& width, const unsigned int& height, const unsigned int& textureid)
{

    glPushAttrib(GL_ALL_ATTRIB_BITS);

    glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
    glDisable(GL_DEPTH_TEST);

    glDisable(GL_LIGHTING);

    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();
    glOrtho(0,width,height,0,-0.1,0.1);
    glViewport(0,0,width,height);

    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glLoadIdentity();

    glBindTexture(GL_TEXTURE_2D,textureid);

    glBegin(GL_QUADS);
    glColor3f(1.0f,1.0f,1.0f);
    glTexCoord2f(0.0f,0.0f);
    glVertex2i(0,0);
    glTexCoord2f(1.0f,0.0f);
    glVertex2i(width,0);
    glTexCoord2f(1.0f,1.0f);
    glVertex2i(width,height);
    glTexCoord2f(0.0f,1.0f);
    glVertex2i(0,height);
    glEnd();

    glBindTexture(GL_TEXTURE_2D,0);

    glMatrixMode(GL_PROJECTION);
    glPopMatrix();
    glMatrixMode(GL_MODELVIEW);
    glPopMatrix();

    glPopAttrib();
}


static void displayGL(void)
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glPushMatrix();

    drawFullScreenQuad(windowsize.width, windowsize.height, frametexid);

    if(calibrated && found)
    {
        hvrl::CVToGLConversion::extrinsicMatrix(R, T, glmodelview);
        hvrl::CVToGLConversion::intrinsicMatrix(K, windowsize, near, far, glprojection);

        glMatrixMode(GL_PROJECTION);
        glPushMatrix();
        glLoadMatrixf(glm::value_ptr(glprojection));


        glMatrixMode(GL_MODELVIEW);
        glPushMatrix();
        glLoadMatrixf(glm::value_ptr(glmodelview));
        glColor3f(1.0,0.0,0.0);

        for(int i = 0; i <= pattern.height; ++i)
        {
            for(int j = 0; j < pattern.width-3; ++j)
            {

                glPushMatrix();
                if(i%2==0)
                {
                    glColor3f(1.0f,0.0f,0.0f);
                    glTranslatef(2.0f*j*squareSize-squareSize/2.0f,i*squareSize-squareSize/2.0f,-squareSize/2.0f);
                }
                else
                {
                    glColor3f(0.0f,0.0f,1.0f);
                    glTranslatef(2.0f*j*squareSize+squareSize/2.0f,i*squareSize-squareSize/2.0f,-squareSize/2.0f);
                }
                glutWireCube(squareSize);

                glPopMatrix();
            }
        }

        glBegin(GL_LINES);
        glColor3f(1.0f,0.0f,0.0f);
        glVertex3f(0.0f,0.0f,0.0f);
        glVertex3f(squareSize,0.0f,0.0f);
        glColor3f(0.0f,1.0f,0.0f);
        glVertex3f(0.0f,0.0f,0.0f);
        glVertex3f(0.0f,squareSize,0.0f);
        glColor3f(0.0f,0.0f,1.0f);
        glVertex3f(0.0f,0.0f,0.0f);
        glVertex3f(0.0f,0.0f,squareSize);

        glEnd();

        glMatrixMode(GL_PROJECTION);
        glPopMatrix();
        glMatrixMode(GL_MODELVIEW);
        glPopMatrix();


    }

#ifdef DEBUG
    GLenum err = glGetError();
    if(err!=GL_NO_ERROR)
    {
        std::cerr << "Une erreur s'est produite : "  << std::endl;
        std::cerr << gluErrorString(err) << std::endl;
    }
#endif

    glPopMatrix();

    glutSwapBuffers();

}

static void reshapeGL(int newwidth,
                      int newheight)
{
    float ratio = (float)newwidth/(float)newheight;
    glViewport(0, 0, (GLint)newwidth, (GLint)newheight);

    windowsize.width = newwidth;
    windowsize.height = newheight;

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(45.0f, ratio, 0.1f, 100.0f);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

}

unsigned int createTexture(const cv::Mat& img, const GLenum& type)
{

    GLuint texid = 0;
    glGenTextures(1,&texid);
    if(texid==0)
    {
        // error("createTexture","unable to create a texture ID");
        return 0;
    }

    glBindTexture(GL_TEXTURE_2D,texid);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
    glTexImage2D (GL_TEXTURE_2D,0,GL_RGB,img.cols,img.rows,0,type,GL_UNSIGNED_BYTE,img.data);
    glBindTexture(GL_TEXTURE_2D,0);
    return texid;

}

void updateTexture(unsigned int& texid, const cv::Mat& img, const GLenum& type)
{

    if(texid == 0)
    {
        texid = createTexture(img, type);
    }
    else
    {
        GLint width,height;
        glGetTexLevelParameteriv(GL_TEXTURE_2D,0,GL_TEXTURE_WIDTH,&width);
        glGetTexLevelParameteriv(GL_TEXTURE_2D,0,GL_TEXTURE_HEIGHT,&height);
        if(img.cols!= width || img.rows!=height)
        {
            glDeleteTextures(1,&texid);
            texid = createTexture(img,type);
        }
        else
        {
            glBindTexture(GL_TEXTURE_2D,texid);
            glTexSubImage2D(GL_TEXTURE_2D,0,GL_RGB,img.cols,img.rows,0,type,GL_UNSIGNED_BYTE,img.data);
            glBindTexture(GL_TEXTURE_2D,0);
        }

    }
}

static void idleGL(void)
{

    cap >> frame;

    std::vector<cv::Point2f> cornerspoints;
    found = cv::findChessboardCorners(frame, pattern, cornerspoints, CV_CALIB_CB_FAST_CHECK|CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_NORMALIZE_IMAGE);

    if(found)
    {
        if(!calibrated)
        {
            cv::TermCriteria criteria = cv::TermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 10, 0.01f);

            cv::Mat gray;
            cvtColor(frame, gray, CV_RGB2GRAY);
            cv::cornerSubPix(gray, cornerspoints, cv::Size(11,11), cv::Size(-1,-1), criteria);
            int dt = glutGet(GLUT_ELAPSED_TIME) - timebase;

            if(dt > 400)
            {
                if(captured++ < 30)
                {
                    points.push_back(cornerspoints);
                    timebase += dt;
                }
                else
                {
                    corners[0].resize(0);
                    for( int i = 0; i < pattern.height; i++ )
                        for( int j = 0; j < pattern.width; j++ )
                            corners[0].push_back(cv::Point3f(float(j*squareSize), float(i*squareSize), 0));


                    std::vector<cv::Mat> rvecs;
                    std::vector<cv::Mat> tvecs;
                    K = cv::Matx33f(1.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,0.0f,1.0f);
                    distCoeffs = cv::Mat::zeros(8, 1, CV_32F);
                    corners.resize(points.size(),corners[0]);
                    calibrateCamera(corners, points, windowsize, K, distCoeffs, rvecs, tvecs, CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);
                    points.clear();
                    cv::Rodrigues(rvecs[0],R);
                    T = tvecs[0];
                    rvecs.clear();
                    tvecs.clear();
                    calibrated = true;
                }
            }
        }
        else
        {
            cv::Mat dist;
            cv::Mat Rtmp;
            cv::solvePnP(corners[0],cornerspoints,K,dist,Rtmp,T);
            cv::Rodrigues(Rtmp,R);
        }
    }
    cv::drawChessboardCorners(frame,pattern,cornerspoints,found);
    updateTexture(frametexid,frame, GL_BGR);
    cornerspoints.clear();
    glutPostRedisplay();
}

static void quit(void)
{
    cap.release();
    glDeleteTextures(1,&frametexid);
    corners.clear();
    isrunning = false;
}

static void keyboardGL(unsigned char c, int x, int y)
{
    switch(c)
    {
    case 27:
    case 'q':
        quit();
        break;
    default:
        break;
    }
    glutPostRedisplay();
}

static bool initGL(int argc,
                   char **argv)
{

    glClearColor(0.8f,0.8f,0.8f,1.0f);

    glDisable(GL_LIGHTING);
    glDisable(GL_LIGHT0);

    glEnable(GL_DEPTH_TEST);

    //glEnable(GL_CULL_FACE);

    glShadeModel(GL_SMOOTH);

    glEnable(GL_TEXTURE_2D);

    glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);

    glPixelStorei(GL_UNPACK_ALIGNMENT,1);
    glPixelStorei(GL_PACK_ALIGNMENT,1);

    cap.open(0);
    cap.set(CV_CAP_PROP_FRAME_WIDTH,windowsize.width);
    cap.set(CV_CAP_PROP_FRAME_HEIGHT,windowsize.height);
    return true;
}

int main(int argc, char **argv)
{
    unsigned int windowid = 0;
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);

    glutInitWindowPosition(0, 0);
    glutInitWindowSize(windowsize.width, windowsize.height);


    if((windowid=glutCreateWindow(" ")) == 0)
        return 1;

    if(!initGL(argc, argv))
        quit();

    glutReshapeFunc(reshapeGL);
    glutDisplayFunc(displayGL);
    glutIdleFunc(idleGL);
    glutKeyboardFunc(keyboardGL);

//glutMainLoop();
    while(isrunning)
    {
        idleGL();
        glutMainLoopEvent();

    }

    glutDestroyWindow(windowid);
    return 0;
}

